<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Icarous: /home/research133/Software/Icarous2/cFS/apps/port_lib/fsw/public_inc/mavlink/mavlink_conversions.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Icarous
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
      <li><a href="globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_c1698e121239b1aba7f6e782e4514e49.html">cFS</a></li><li class="navelem"><a class="el" href="dir_7ee4ce95da95d11e80a01c5101ed8d14.html">apps</a></li><li class="navelem"><a class="el" href="dir_414860549fe1f5475bcb32699266eda6.html">port_lib</a></li><li class="navelem"><a class="el" href="dir_96eb22f570806912b0c00e39297e594c.html">fsw</a></li><li class="navelem"><a class="el" href="dir_177fe1f9cf5443c270b5fea5afe4fbfe.html">public_inc</a></li><li class="navelem"><a class="el" href="dir_21e8b98476f1332e3b94242d590432e6.html">mavlink</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">mavlink_conversions.h</div>  </div>
</div><!--header-->
<div class="contents">
<a href="mavlink__conversions_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="preprocessor">#pragma once</span></div><div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;</div><div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="preprocessor">#ifndef MAVLINK_NO_CONVERSION_HELPERS</span></div><div class="line"><a name="l00004"></a><span class="lineno">    4</span>&#160;</div><div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;<span class="comment">/* enable math defines on Windows */</span></div><div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="preprocessor">#ifdef _MSC_VER</span></div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="preprocessor">#ifndef _USE_MATH_DEFINES</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#define _USE_MATH_DEFINES</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#endif</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="preprocessor">#endif</span></div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="preprocessor">#include &lt;math.h&gt;</span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;</div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="preprocessor">#ifndef M_PI_2</span></div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;<span class="preprocessor">    #define M_PI_2 ((float)asin(1))</span></div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="preprocessor">#endif</span></div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;</div><div class="line"><a name="l00039"></a><span class="lineno"><a class="line" href="mavlink__conversions_8h.html#a1acebd1df006ab806149f3bf9cc5544e">   39</a></span>&#160;MAVLINK_HELPER <span class="keywordtype">void</span> <a class="code" href="mavlink__conversions_8h.html#a1acebd1df006ab806149f3bf9cc5544e">mavlink_quaternion_to_dcm</a>(<span class="keyword">const</span> <span class="keywordtype">float</span> quaternion[4], <span class="keywordtype">float</span> dcm[3][3])</div><div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;{</div><div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;    <span class="keywordtype">double</span> a = quaternion[0];</div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;    <span class="keywordtype">double</span> b = quaternion[1];</div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;    <span class="keywordtype">double</span> c = quaternion[2];</div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;    <span class="keywordtype">double</span> d = quaternion[3];</div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;    <span class="keywordtype">double</span> aSq = a * a;</div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;    <span class="keywordtype">double</span> bSq = b * b;</div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;    <span class="keywordtype">double</span> cSq = c * c;</div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;    <span class="keywordtype">double</span> dSq = d * d;</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;    dcm[0][0] = aSq + bSq - cSq - dSq;</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;    dcm[0][1] = 2 * (b * c - a * d);</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;    dcm[0][2] = 2 * (a * c + b * d);</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;    dcm[1][0] = 2 * (b * c + a * d);</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;    dcm[1][1] = aSq - bSq + cSq - dSq;</div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;    dcm[1][2] = 2 * (c * d - a * b);</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;    dcm[2][0] = 2 * (b * d - a * c);</div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;    dcm[2][1] = 2 * (a * b + c * d);</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;    dcm[2][2] = aSq - bSq - cSq + dSq;</div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;}</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;</div><div class="line"><a name="l00069"></a><span class="lineno"><a class="line" href="mavlink__conversions_8h.html#a034c8265f27e81fa17cf4cf483b7ad8c">   69</a></span>&#160;MAVLINK_HELPER <span class="keywordtype">void</span> <a class="code" href="mavlink__conversions_8h.html#a034c8265f27e81fa17cf4cf483b7ad8c">mavlink_dcm_to_euler</a>(<span class="keyword">const</span> <span class="keywordtype">float</span> dcm[3][3], <span class="keywordtype">float</span>* roll, <span class="keywordtype">float</span>* pitch, <span class="keywordtype">float</span>* yaw)</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;{</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;    <span class="keywordtype">float</span> phi, theta, psi;</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;    theta = asin(-dcm[2][0]);</div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;    <span class="keywordflow">if</span> (fabsf(theta - (<span class="keywordtype">float</span>)M_PI_2) &lt; 1.0e-3f) {</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;        phi = 0.0f;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;        psi = (atan2f(dcm[1][2] - dcm[0][1],</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;                dcm[0][2] + dcm[1][1]) + phi);</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;    } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (fabsf(theta + (<span class="keywordtype">float</span>)M_PI_2) &lt; 1.0e-3f) {</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;        phi = 0.0f;</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;        psi = atan2f(dcm[1][2] - dcm[0][1],</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;                  dcm[0][2] + dcm[1][1] - phi);</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;    } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;        phi = atan2f(dcm[2][1], dcm[2][2]);</div><div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;        psi = atan2f(dcm[1][0], dcm[0][0]);</div><div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;    }</div><div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;</div><div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;    *roll = phi;</div><div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;    *pitch = theta;</div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;    *yaw = psi;</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;}</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;</div><div class="line"><a name="l00103"></a><span class="lineno"><a class="line" href="mavlink__conversions_8h.html#a96913fc5846ae95f89c99423e4c0e6c1">  103</a></span>&#160;MAVLINK_HELPER <span class="keywordtype">void</span> <a class="code" href="mavlink__conversions_8h.html#a96913fc5846ae95f89c99423e4c0e6c1">mavlink_quaternion_to_euler</a>(<span class="keyword">const</span> <span class="keywordtype">float</span> quaternion[4], <span class="keywordtype">float</span>* roll, <span class="keywordtype">float</span>* pitch, <span class="keywordtype">float</span>* yaw)</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;{</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;    <span class="keywordtype">float</span> dcm[3][3];</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;    <a class="code" href="mavlink__conversions_8h.html#a1acebd1df006ab806149f3bf9cc5544e">mavlink_quaternion_to_dcm</a>(quaternion, dcm);</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;    <a class="code" href="mavlink__conversions_8h.html#a034c8265f27e81fa17cf4cf483b7ad8c">mavlink_dcm_to_euler</a>((<span class="keyword">const</span> <span class="keywordtype">float</span>(*)[3])dcm, roll, pitch, yaw);</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;}</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;</div><div class="line"><a name="l00119"></a><span class="lineno"><a class="line" href="mavlink__conversions_8h.html#a286e44d134e0722c8fbc595f9d25d767">  119</a></span>&#160;MAVLINK_HELPER <span class="keywordtype">void</span> <a class="code" href="mavlink__conversions_8h.html#a286e44d134e0722c8fbc595f9d25d767">mavlink_euler_to_quaternion</a>(<span class="keywordtype">float</span> roll, <span class="keywordtype">float</span> pitch, <span class="keywordtype">float</span> yaw, <span class="keywordtype">float</span> quaternion[4])</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;{</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;    <span class="keywordtype">float</span> cosPhi_2 = cosf(roll / 2);</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;    <span class="keywordtype">float</span> sinPhi_2 = sinf(roll / 2);</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;    <span class="keywordtype">float</span> cosTheta_2 = cosf(pitch / 2);</div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;    <span class="keywordtype">float</span> sinTheta_2 = sinf(pitch / 2);</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;    <span class="keywordtype">float</span> cosPsi_2 = cosf(yaw / 2);</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;    <span class="keywordtype">float</span> sinPsi_2 = sinf(yaw / 2);</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;    quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;            sinPhi_2 * sinTheta_2 * sinPsi_2);</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;    quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;            cosPhi_2 * sinTheta_2 * sinPsi_2);</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;    quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;            sinPhi_2 * cosTheta_2 * sinPsi_2);</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;    quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;            sinPhi_2 * sinTheta_2 * cosPsi_2);</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;}</div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;</div><div class="line"><a name="l00147"></a><span class="lineno"><a class="line" href="mavlink__conversions_8h.html#a4e183fc5dc505bee17dfac58ab18d9f9">  147</a></span>&#160;MAVLINK_HELPER <span class="keywordtype">void</span> <a class="code" href="mavlink__conversions_8h.html#a4e183fc5dc505bee17dfac58ab18d9f9">mavlink_dcm_to_quaternion</a>(<span class="keyword">const</span> <span class="keywordtype">float</span> dcm[3][3], <span class="keywordtype">float</span> quaternion[4])</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;{</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;    <span class="keywordtype">float</span> tr = dcm[0][0] + dcm[1][1] + dcm[2][2];</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;    <span class="keywordflow">if</span> (tr &gt; 0.0f) {</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;        <span class="keywordtype">float</span> s = sqrtf(tr + 1.0f);</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;        quaternion[0] = s * 0.5f;</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;        s = 0.5f / s;</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;        quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;        quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;        quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;    } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;        <span class="comment">/* Find maximum diagonal element in dcm</span></div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;<span class="comment">         * store index in dcm_i */</span></div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;        <span class="keywordtype">int</span> dcm_i = 0;</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;        <span class="keywordtype">int</span> i;</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;        <span class="keywordflow">for</span> (i = 1; i &lt; 3; i++) {</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;            <span class="keywordflow">if</span> (dcm[i][i] &gt; dcm[dcm_i][dcm_i]) {</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;                dcm_i = i;</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;            }</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;        }</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;        <span class="keywordtype">int</span> dcm_j = (dcm_i + 1) % 3;</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;        <span class="keywordtype">int</span> dcm_k = (dcm_i + 2) % 3;</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;        <span class="keywordtype">float</span> s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;                    dcm[dcm_k][dcm_k]) + 1.0f);</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;        quaternion[dcm_i + 1] = s * 0.5f;</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;        s = 0.5f / s;</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;        quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;        quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;        quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;    }</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;}</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;</div><div class="line"><a name="l00190"></a><span class="lineno"><a class="line" href="mavlink__conversions_8h.html#a5a662d9d3db2fb392c9466dab354b287">  190</a></span>&#160;MAVLINK_HELPER <span class="keywordtype">void</span> <a class="code" href="mavlink__conversions_8h.html#a5a662d9d3db2fb392c9466dab354b287">mavlink_euler_to_dcm</a>(<span class="keywordtype">float</span> roll, <span class="keywordtype">float</span> pitch, <span class="keywordtype">float</span> yaw, <span class="keywordtype">float</span> dcm[3][3])</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;{</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;    <span class="keywordtype">float</span> cosPhi = cosf(roll);</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;    <span class="keywordtype">float</span> sinPhi = sinf(roll);</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;    <span class="keywordtype">float</span> cosThe = cosf(pitch);</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;    <span class="keywordtype">float</span> sinThe = sinf(pitch);</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;    <span class="keywordtype">float</span> cosPsi = cosf(yaw);</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;    <span class="keywordtype">float</span> sinPsi = sinf(yaw);</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;    dcm[0][0] = cosThe * cosPsi;</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;    dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;    dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;    dcm[1][0] = cosThe * sinPsi;</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;    dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;    dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;</div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;    dcm[2][0] = -sinThe;</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;    dcm[2][1] = sinPhi * cosThe;</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;    dcm[2][2] = cosPhi * cosThe;</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;}</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;<span class="preprocessor">#endif // MAVLINK_NO_CONVERSION_HELPERS</span></div><div class="ttc" id="mavlink__conversions_8h_html_a5a662d9d3db2fb392c9466dab354b287"><div class="ttname"><a href="mavlink__conversions_8h.html#a5a662d9d3db2fb392c9466dab354b287">mavlink_euler_to_dcm</a></div><div class="ttdeci">MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])</div><div class="ttdef"><b>Definition:</b> mavlink_conversions.h:190</div></div>
<div class="ttc" id="mavlink__conversions_8h_html_a034c8265f27e81fa17cf4cf483b7ad8c"><div class="ttname"><a href="mavlink__conversions_8h.html#a034c8265f27e81fa17cf4cf483b7ad8c">mavlink_dcm_to_euler</a></div><div class="ttdeci">MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float *roll, float *pitch, float *yaw)</div><div class="ttdef"><b>Definition:</b> mavlink_conversions.h:69</div></div>
<div class="ttc" id="mavlink__conversions_8h_html_a96913fc5846ae95f89c99423e4c0e6c1"><div class="ttname"><a href="mavlink__conversions_8h.html#a96913fc5846ae95f89c99423e4c0e6c1">mavlink_quaternion_to_euler</a></div><div class="ttdeci">MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float *roll, float *pitch, float *yaw)</div><div class="ttdef"><b>Definition:</b> mavlink_conversions.h:103</div></div>
<div class="ttc" id="mavlink__conversions_8h_html_a1acebd1df006ab806149f3bf9cc5544e"><div class="ttname"><a href="mavlink__conversions_8h.html#a1acebd1df006ab806149f3bf9cc5544e">mavlink_quaternion_to_dcm</a></div><div class="ttdeci">MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])</div><div class="ttdef"><b>Definition:</b> mavlink_conversions.h:39</div></div>
<div class="ttc" id="mavlink__conversions_8h_html_a286e44d134e0722c8fbc595f9d25d767"><div class="ttname"><a href="mavlink__conversions_8h.html#a286e44d134e0722c8fbc595f9d25d767">mavlink_euler_to_quaternion</a></div><div class="ttdeci">MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])</div><div class="ttdef"><b>Definition:</b> mavlink_conversions.h:119</div></div>
<div class="ttc" id="mavlink__conversions_8h_html_a4e183fc5dc505bee17dfac58ab18d9f9"><div class="ttname"><a href="mavlink__conversions_8h.html#a4e183fc5dc505bee17dfac58ab18d9f9">mavlink_dcm_to_quaternion</a></div><div class="ttdeci">MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])</div><div class="ttdef"><b>Definition:</b> mavlink_conversions.h:147</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.11
</small></address>
</body>
</html>
